# pragma once

# include "cpp_tools.h"
# include "filePathSettings.h"
# include "XmlParameterManager.h"

namespace data
{
    struct Frenet
    {
        double s; // 曲线距离 m
        double e; // 横向距离 m
        double Delta_phi; // 速度航向误差 rad
    };

    struct Cartesian
    {
        double x; // X坐标 m
        double y; // Y坐标 m
        double varphi; // 航向 rad
    };

    struct Dynamic
    {
        double v; // 质心处的速度 m/s
        double beta; // 质心侧偏角 rad
        double omega; // 横摆角速度 rad/s
        double a_x; // 车身X方向加速度 m/s^2
        double a_y; // 车身Y方向加速度 m/s^2
    };

    struct road
    {
    public:
        int N;

        Eigen::VectorXd x;
        Eigen::VectorXd y;
        Eigen::VectorXd varphi;

        Eigen::VectorXd S;
        Eigen::VectorXd curve;

        // 3D info
        Eigen::VectorXd psi;
        Eigen::VectorXd gamma;
    };

    struct trajectory : public road
    {
    public:
        Eigen::VectorXd v_ref;
        Eigen::VectorXd beta_ref;
        Eigen::VectorXd e_ref;
        Eigen::VectorXd Delta_phi_ref;
    };

    struct vehicle_param
    {
        struct body
        {
            double mass_sprung = 5.925;
            double mass_unsprung_front = 0.001;
            double mass_unsprung_rear = 0.001;
            double inertial_z = 0.12;
            double l_f = 0.192;
            double l_r = 0.193;
            double l_y = 0.262;
            double h_cg = 0.12;
        } m_body;
        
        struct tire
        {
            double R_wheel = 0.047;
            double mass_wheel = 115e-3;
            double inertial_spin = 0.0003;
            
            struct MF
            {
                double B;
                double C;
                double D;
                double E;
            } MF_x = {3.289, 1.0901, 0.9, 0}, MF_y = {3.289, 1.0901, 0.9, 0};
        } m_tire;
        
        struct common
        {
            double g = 9.81;
            double suspension_response = 0.05;
            double motor_torque_max = 80/4*0.047;
            double break_torque_max = 80/4*0.047;
            double front_steer_max = 0.52;
            double rear_steer_max = 0;
        } m_common;

        /// @brief 初始化函数
        vehicle_param()
        {
            readXML();
        }

        /// @brief 从xml文件中读取参数设置
        /// @param gen 如果找不到文件是否新建一个
        /// @return 
        void readXML(bool gen = 1)
        {
            std::string path = host_path + settings_path + "/vehicle/";
            std::string filename = "param.xml";

            cpp_tools::checkPath(path);
            if (!cpp_tools::checkFile(path + filename) && gen)
            {
                genXML(path, filename);

                return; // 由于生成的就是默认参数，因此不再重新读取
            }

            cpp_tools::XmlParameterManager m_xml("veh_param", path + filename);

            m_xml.getParameter("/body", "mass_sprung", m_body.mass_sprung);
            m_xml.getParameter("/body", "mass_unsprung_front", m_body.mass_unsprung_front);
            m_xml.getParameter("/body", "mass_unsprung_rear", m_body.mass_unsprung_rear);
            m_xml.getParameter("/body", "inertial_z", m_body.inertial_z);
            m_xml.getParameter("/body", "l_f", m_body.l_f);
            m_xml.getParameter("/body", "l_r", m_body.l_r);
            m_xml.getParameter("/body", "l_y", m_body.l_y);
            m_xml.getParameter("/body", "h_cg", m_body.h_cg);

            m_xml.getParameter("/tire", "R_wheel", m_tire.R_wheel);
            m_xml.getParameter("/tire", "mass_wheel", m_tire.mass_wheel);
            m_xml.getParameter("/tire", "inertial_spin", m_tire.inertial_spin);
            m_xml.getParameter("/tire/MF_x", "B", m_tire.MF_x.B);
            m_xml.getParameter("/tire/MF_x", "C", m_tire.MF_x.C);
            m_xml.getParameter("/tire/MF_x", "D", m_tire.MF_x.D);
            m_xml.getParameter("/tire/MF_x", "E", m_tire.MF_x.E);
            m_xml.getParameter("/tire/MF_y", "B", m_tire.MF_y.B);
            m_xml.getParameter("/tire/MF_y", "C", m_tire.MF_y.C);
            m_xml.getParameter("/tire/MF_y", "D", m_tire.MF_y.D);
            m_xml.getParameter("/tire/MF_y", "E", m_tire.MF_y.E);

            m_xml.getParameter("/common", "g", m_common.g);
            m_xml.getParameter("/common", "suspension_response", m_common.suspension_response);
            m_xml.getParameter("/common", "motor_torque_max", m_common.motor_torque_max);
            m_xml.getParameter("/common", "break_torque_max", m_common.break_torque_max);
            m_xml.getParameter("/common", "front_steer_max", m_common.front_steer_max);
            m_xml.getParameter("/common", "rear_steer_max", m_common.rear_steer_max);
        }

        /// @brief 生成XML参数文件
        /// @param cover 是否覆盖原有文件
        void genXML(std::string path, std::string filename, bool cover = 0)
        {
            cpp_tools::checkPath(path);
            if (cpp_tools::checkFile(path + filename) && !cover)
            {
                return; // 如果文件存在且不覆盖，则直接返回
            }

            cpp_tools::XmlParameterManager m_xml("veh_param");
            
            m_xml.setParameter("/body", "mass_sprung", m_body.mass_sprung);
            m_xml.setParameter("/body", "mass_unsprung_front", m_body.mass_unsprung_front);
            m_xml.setParameter("/body", "mass_unsprung_rear", m_body.mass_unsprung_rear);
            m_xml.setParameter("/body", "inertial_z", m_body.inertial_z);
            m_xml.setParameter("/body", "l_f", m_body.l_f);
            m_xml.setParameter("/body", "l_r", m_body.l_r);
            m_xml.setParameter("/body", "l_y", m_body.l_y);
            m_xml.setParameter("/body", "h_cg", m_body.h_cg);

            m_xml.setParameter("/tire", "R_wheel", m_tire.R_wheel);
            m_xml.setParameter("/tire", "mass_wheel", m_tire.mass_wheel);
            m_xml.setParameter("/tire", "inertial_spin", m_tire.inertial_spin);
            m_xml.setParameter("/tire/MF_x", "B", m_tire.MF_x.B);
            m_xml.setParameter("/tire/MF_x", "C", m_tire.MF_x.C);
            m_xml.setParameter("/tire/MF_x", "D", m_tire.MF_x.D);
            m_xml.setParameter("/tire/MF_x", "E", m_tire.MF_x.E);
            m_xml.setParameter("/tire/MF_y", "B", m_tire.MF_y.B);
            m_xml.setParameter("/tire/MF_y", "C", m_tire.MF_y.C);
            m_xml.setParameter("/tire/MF_y", "D", m_tire.MF_y.D);
            m_xml.setParameter("/tire/MF_y", "E", m_tire.MF_y.E);

            m_xml.setParameter("/common", "g", m_common.g);
            m_xml.setParameter("/common", "suspension_response", m_common.suspension_response);
            m_xml.setParameter("/common", "motor_torque_max", m_common.motor_torque_max);
            m_xml.setParameter("/common", "break_torque_max", m_common.break_torque_max);
            m_xml.setParameter("/common", "front_steer_max", m_common.front_steer_max);
            m_xml.setParameter("/common", "rear_steer_max", m_common.rear_steer_max);

            m_xml.saveToFile(path + filename);
        }
    };

    namespace communicate
    {
        namespace lower_solver
        {
            enum class mpc_order
            {
                solve = 1,
                refresh = 2,
            };

            enum class mpc_v2_order
            {
                update = 1,
                getControl = 2,
                refresh = 3,
            };
        } // namespace lower_solver
    } // namespace communicate
}
